/*
frigOS
Licensed under GPLv3

Functions and definitions for interacting with Dynamixel AX-12 servo motors
Servo motors are connected to USART0 using a half-duplex communication line
USART0 should be initialized before attempting to communicate with the servos
*/

#ifndef DISABLE_AX12
#ifndef __AX12MOTOR_H__
#define __AX12MOTOR_H__

#include <stdint.h>
#include "defs.h"
#include "dynamixel.h"

// AX-12 address table
enum
{
    AX12_MODEL_NUMBER_L            = 0x00,
    AX12_MODEL_NUMBER_H            = 0x01,
    AX12_VERSION_OF_FIRMWARE       = 0x02,
    AX12_ID                        = 0x03,
    AX12_BAUD_RATE                 = 0x04,
    AX12_RETURN_DELAY_TIME         = 0x05,
    AX12_CW_ANGLE_LIMIT_L          = 0x06,
    AX12_CW_ANGLE_LIMIT_H          = 0x07,
    AX12_CCW_ANGLE_LIMIT_L         = 0x08,
    AX12_CCW_ANGLE_LIMIT_H         = 0x09,
    AX12_HIGHEST_LIMIT_TEMPERATURE = 0x0b,
    AX12_LOWEST_LIMIT_VOLTAGE      = 0x0c,
    AX12_HIGHEST_LIMIT_VOLTAGE     = 0x0d,
    AX12_MAX_TORQUE_L              = 0x0e,
    AX12_MAX_TORQUE_H              = 0x0f,
    AX12_STATUS_RETURN_LEVEL       = 0x10,
    AX12_ALARM_LED                 = 0x11,
    AX12_ALARM_SHUTDOWN            = 0x12,
    AX12_DOWN_CALIBRATION_L        = 0x14,
    AX12_DOWN_CALIBRATION_H        = 0x15,
    AX12_UP_CALIBRATION_L          = 0x16,
    AX12_UP_CALIBRATION_H          = 0x17,
    AX12_TORQUE_ENABLE             = 0x18,
    AX12_LED                       = 0x19,
    AX12_CW_COMPLIANCE_MARGIN      = 0x1a,
    AX12_CCW_COMPLIANCE_MARGIN     = 0x1b,
    AX12_CW_COMPLIANCE_SLOPE       = 0x1c,
    AX12_CCW_COMPLIANCE_SLOPE      = 0x1d,
    AX12_GOAL_POSITION_L           = 0x1e,
    AX12_GOAL_POSITION_H           = 0x1f,
    AX12_MOVING_SPEED_L            = 0x20,
    AX12_MOVING_SPEED_H            = 0x21,
    AX12_TORQUE_LIMIT_L            = 0x22,
    AX12_TORQUE_LIMIT_H            = 0x23,
    AX12_PRESENT_POSITION_L        = 0x24,
    AX12_PRESENT_POSITION_H        = 0x25,
    AX12_PRESENT_SPEED_L           = 0x26,
    AX12_PRESENT_SPEED_H           = 0x27,
    AX12_PRESENT_LOAD_L            = 0x28,
    AX12_PRESENT_LOAD_H            = 0x29,
    AX12_PRESENT_VOLTAGE           = 0x2a,
    AX12_PRESENT_TEMPERATURE       = 0x2b,
    AX12_REGISTERED_INSTRUCTION    = 0x2c,
    AX12_MOVING                    = 0x2e,
    AX12_LOCK                      = 0x2f,
    AX12_PUNCH_L                   = 0x30,
    AX12_PUNCH_H                   = 0x31
};



// maximum servos connected to the CM-5
// this may vary depending on the robot configuration
#define NUMBER_OF_SERVOS                    20

void initServo(uint8_t id);
uint16_t getPosition(uint8_t id);
uint16_t getLoad(uint8_t id);
uint16_t getSpeed(uint8_t id);
void setGoalPosition(uint8_t id, uint16_t position);
void setSpeed(uint8_t id, uint16_t speed);
void syncSetPositionAndSpeed(uint8_t const positions[], uint16_t speed);


#ifdef HW_DEBUG
// enter a busy-loop mode that will simply allow the user to interactively test the motors
void testMotors(void);
#endif


#endif  //__AX12MOTOR_H__
#endif  //DISABLE_AX12
